#include <render.h>
#include <transforms.h>
#include <math.h>

#include <debug.h>
#include <assert.h>

static const double ACCEPTED_ERROR = 0.75;


static inline color_t
color_from_voxel( const model3d_t *model,
		  const vector3_t pos,
		  const uint16_t vmin, const uint16_t vmax )
{
  assert( model != NULL );
  assert( vmin <= vmax );
  assert( VECTOR3_X( pos ) >= 0 );
  assert( VECTOR3_Y( pos ) >= 0 );
  assert( VECTOR3_Z( pos ) >= 0 );
  assert( VECTOR3_X( pos ) < model->size.x );
  assert( VECTOR3_Y( pos ) < model->size.y );
  assert( VECTOR3_Z( pos ) < model->size.z );

  uint16_t voxel = VOXEL_AT( model,
			     (uint16_t)VECTOR3_X( pos ),
			     (uint16_t)VECTOR3_Y( pos ),
			     (uint16_t)VECTOR3_Z( pos ) );

  voxel = min( voxel, vmax );
  voxel = max( voxel, vmin );
  uint8_t v = 255.0 * ((float)(voxel - vmin)) / vmax;
  color_t c = { v, v, v };
  return c;
}

void
find_intensities( const model3d_t *model, uint16_t *vmin, uint16_t *vmax )
{
  assert( model != NULL );
  assert( vmax != NULL );
  assert( vmin != NULL );
  *vmax = 0;
  *vmin = -1;

  uint16_t *p, *pend;

  pend = model->buffer + model->size.x * model->size.y * model->size.z;

  for ( p=model->buffer; p < pend; p++ )
    {
      *vmax = max( *vmax, *p );
      *vmin = min( *vmin, *p );
    }
}

static inline vector3_t
calc_n( const double tetha_y, const double tetha_x )
{
  matrix4x4_t *pipeline[] = {
    rotate_x( -tetha_x ),
    rotate_y( -tetha_y ),
    NULL,
  };
  matrix4x4_t *t = compile_pipeline( pipeline );
  free_pipeline_matrices( pipeline );

  vector3_t n;
  multiply_matrix4x4_vector3( *t, vector3_init( 0, 0, -1 ), &n );

  matrix4x4_free( t );
  return vector3_normalize( n );
}

static inline int
edge( const plane_t *planes,
      const unsigned int n_planes,
      const unsigned int n_visible,
      const vector3_t n,
      const vector3_t p )
{
  unsigned i;
  int count = 0, visible = 0;
  vector3_t r;

  for ( i=0; i < n_planes; i++ )
    {
      if ( ! plane_intercept( planes[ i ], p, planes[ i ].normal, &r ) )
	continue;

      /* point from this plane is the same as point in other plane */
      if ( double_is_equal( VECTOR3_X( p ), VECTOR3_X( r ), ACCEPTED_ERROR ) &&
	   double_is_equal( VECTOR3_Y( p ), VECTOR3_Y( r ), ACCEPTED_ERROR ) &&
	   double_is_equal( VECTOR3_Z( p ), VECTOR3_Z( r ), ACCEPTED_ERROR ) )
	{
	  count ++;
	  if ( i < n_visible )
	    visible ++;

	  if ( ( count > 1 ) && ( visible ) )
	    return 1;
	}
    }

  return 0;
}

static inline void
fixate( vector3_t *pos_mod )
{
  /* fix values like -0.00001 */
  if ( double_is_equal( VECTOR3_X( *pos_mod ), 0, DOUBLE_PRECISION ) )
    VECTOR3_X( *pos_mod ) = 0;
  if ( double_is_equal( VECTOR3_Y( *pos_mod ), 0, DOUBLE_PRECISION ) )
    VECTOR3_Y( *pos_mod ) = 0;
  if ( double_is_equal( VECTOR3_Z( *pos_mod ), 0, DOUBLE_PRECISION ) )
    VECTOR3_Z( *pos_mod ) = 0;
}

static inline int
inside( const model3d_t *model,
	const vector3_t pos_mod,
	const plane_t *planes, const unsigned int n_planes,
	const vector3_t pos_scn )
{
  /* must be inside model */
  if ( ( VECTOR3_X( pos_mod ) < 0 ) ||
       ( VECTOR3_Y( pos_mod ) < 0 ) ||
       ( VECTOR3_Z( pos_mod ) < 0 ) ||
       ( VECTOR3_X( pos_mod ) >= model->size.x ) ||
       ( VECTOR3_Y( pos_mod ) >= model->size.y ) ||
       ( VECTOR3_Z( pos_mod ) >= model->size.z ) )
    return 0;

  /* must be inside planes */
  unsigned i;
  for ( i=0; i < n_planes; i++ )
    {
      vector3_t v = vector3_sub( planes[ i ].point, pos_scn );
      double d = vector3_dot( planes[ i ].normal, v );
      /* It's below plane? */
      if ( d < - ACCEPTED_ERROR )
	return 0;
    }

  return 1;
}

static inline matrix4x4_t *
calc_surface_to_camera( const uint16_t width,
			const uint16_t height,
			const double tetha_y,
			const double tetha_x )
{
  int mid_x = width / 2;
  int mid_y = height / 2;

  matrix4x4_t *pipeline_surface_to_camera[] = {
    rotate_x( -tetha_x ),
    rotate_y( -tetha_y ),
    scale( vector3_init( 1, -1, 1 ) ),
    translate( vector3_init( -mid_x, -mid_y, 0 ) ),
    NULL,
  };

  matrix4x4_t *surface_to_camera = \
    compile_pipeline( pipeline_surface_to_camera );

  free_pipeline_matrices( pipeline_surface_to_camera );

  return surface_to_camera;
}

static inline matrix4x4_t *
calc_scene_to_model( const model3d_t *model )
{
  matrix4x4_t *pipeline_scene_to_model[] = {
    translate( vector3_init( 0, -1, -1 ) ), /* count from 0 */
    scale( vector3_init( 1, -1, -1 ) ),     /* axis count downwards */
    translate( vector3_init(  (int)model->size.x / 2,
			     -(int)model->size.y / 2,
			     -(int)model->size.z / 2) ),
    NULL,
  };
  matrix4x4_t *scene_to_model = \
    compile_pipeline( pipeline_scene_to_model );
  free_pipeline_matrices( pipeline_scene_to_model );

  return scene_to_model;
}


struct plane_visibility
{
  plane_t plane;
  double  angle;
};

static int
cmp_plane_visibility( const void *a, const void *b )
{
  const struct plane_visibility c = *(struct plane_visibility*)a;
  const struct plane_visibility d = *(struct plane_visibility*)b;

  if ( c.angle < d.angle )
    return -1;
  else if ( c.angle > d.angle )
    return 1;
  else
    return 0;
}

/**
 * Order planes by visibility. Put most visible at begin, so our hit rate
 * is high in loops to paint pixels.
 *
 * Sets n_visible to number of elements visible.
 */
static inline plane_t *
order_planes( const plane_t *planes, const unsigned int n_planes,
	      const vector3_t normal, unsigned int *n_visible )
{
  assert( planes != NULL );
  assert( n_visible != NULL );

  struct plane_visibility *pv = calloc( n_planes, sizeof( *pv ) );

  unsigned i;
  for ( i=0; i < n_planes; i++ )
    {
      pv[ i ].plane = planes[ i ];
      pv[ i ].angle = vector3_dot( normal, planes[ i ].normal );
    }
  qsort( pv, n_planes, sizeof( *pv ), cmp_plane_visibility );

  plane_t *ord_planes = calloc( n_planes, sizeof( plane_t ) );
  *n_visible = 0;
  for ( i=0; i < n_planes; i++ )
    {
      ord_planes[ i ] = pv[ i ].plane;
      if ( pv[ i ].angle < 0 )
	(*n_visible) ++;
    }

  free( pv );
  return ord_planes;
}


/**
 * Renders 3d-model to a 2d-surfaces.
 *
 * @param model 3d-voxel model to render.
 * @param planes array of planes that clip model.
 * @param n_planes number of planes in array 'planes'.
 * @param rot_y rotation around y axis (horizontal rotation).
 * @param rot_x rotation around x axis (vertical rotation).
 * @param edge_color pointer to color, if NULL, no color is painted.
 * @param vmin minimum voxel intensity to use.
 * @param vmax maximum voxel intensity to use.
 */
void
render( surface_t *surface,
	const uint16_t x0,
	const uint16_t y0,
	const model3d_t *model,
	const plane_t *planes,
	const unsigned int n_planes,
	const double rot_y,
	const double rot_x,
	const color_t *edge_color,
	const color_t *bg_color,
	const uint16_t vmin,
	const uint16_t vmax )
{
  assert( surface != NULL );
  assert( planes != NULL );
  assert( model != NULL );

  uint16_t diagonal3d = ceil( sqrt( model->size.x * model->size.x +
				    model->size.y * model->size.y +
				    model->size.z * model->size.z ) );

  uint16_t max_i = min( (uint16_t)(surface->size.x - x0), diagonal3d );
  uint16_t max_j = min( (uint16_t)(surface->size.y - y0), diagonal3d );

  /* transformation matrices */
  matrix4x4_t *surface_to_camera = \
    calc_surface_to_camera( max_i, max_j, rot_y, rot_x );
  matrix4x4_t *scene_to_model = calc_scene_to_model( model );

  vector3_t n = calc_n( rot_y, rot_x );

  unsigned int n_visible;
  plane_t *ord_planes = order_planes( planes, n_planes, n, &n_visible );

  /* render it! */
  uint16_t i, j;
  for ( i=0; i < max_i; i++ )
    for ( j=0; j < max_j; j++ )
      {
	vector3_t pos_surf = vector3_init( i, j, 0 );
	vector3_t pos_cam;
	multiply_matrix4x4_vector3( *surface_to_camera,
				    pos_surf, &pos_cam );
	int vis;
	int painted = 0;
	for ( vis=0; vis < n_visible; vis++ )
	  {
	    vector3_t pos_scn;
	    if ( plane_intercept( ord_planes[ vis ],
				  pos_cam, n,
				  &pos_scn ) )
	      {
		vector3_t pos_mod;
		multiply_matrix4x4_vector3( *scene_to_model, pos_scn,
					    &pos_mod );

		fixate( &pos_mod );
		if ( inside( model, pos_mod, planes, n_planes, pos_scn ) )
		  {
		    color_t c;
		    if ( edge_color &&
			 edge( ord_planes, n_planes, n_visible, n, pos_scn ) )
		      c = *edge_color;
		    else
		      c = color_from_voxel( model, pos_mod, vmin, vmax );

		    PIXEL_AT( surface, i + x0, j + y0 ) = c;
		    painted = 1;
		    break;
		  }
	      }
	  }
	if ( ( ! painted ) && ( bg_color ) )
	  PIXEL_AT( surface, i + x0, j + y0 ) = *bg_color;
      }

  matrix4x4_free( surface_to_camera );
  matrix4x4_free( scene_to_model );

  free( ord_planes );
}
